#include "LNP.h"

LNP::LNP(){
}

LNP::~LNP(){
	cost.clear();
}

void LNP::clear(){
	cmx = -INF;
	cnt = 0;
	cost.clear();
}

void LNP::make_matrix(int height, int width){
	for( int i=0; i<height; i++ )
		cost.push_back( VECTOR(width,INF) );
}

void LNP::calculate_obs_cost(MATRIX &grid){
	cnt = 0;
	h = grid.size();
	w = grid[0].size();

	clear();
	make_matrix(h,w);
	std::queue<TFII> Q;
	int cc = 0;
	for( int i=0; i<h; i++ ){
		for( int j=0; j<w; j++ ){
			if( grid[i][j] == WALL ){
				Q.push(TFII(0,PII(i,j)));
				cost[i][j] = 0;
				cc++;
			}
		}
	}

	while( !Q.empty() ){
		TFII t = Q.front();
		Q.pop();
		cmx = std::max(cmx,cost[t.second.first][t.second.second]);
		push_neighbors(t.second.first, t.second.second, t.first, Q, grid );
	}

	std::cout << "Count of visits " << cnt << " " << cc << " " << cmx << std::endl;
	/**
	for(int i=0;i<h;i++){
		for(int j=0;j<w;j++){
			printf("%2.1f ",cost[i][j]);
		}std::cout << std::endl;
	}**/
}

void LNP::push_neighbors(int x, int y, double c, std::queue<TFII> &Q, MATRIX &grid ){
	int nx, ny;
	int dx[] = { 0, 0, -1, +1 };
	int dy[] = { -1, +1, 0, 0 };
	double cc;

	for( int i=0; i<4; i++){
		nx = x + dx[i];
		ny = y + dy[i];

		if( nx >= 0 && nx < h
		 && ny >= 0 && ny < w
		 && grid[ nx ][ ny ] != WALL
		 && cost[ nx ][ ny ] == INF
		 ){
			assert(cost[nx][ny]!=0.0);
			cc = update_cost(nx,ny,c);
			Q.push( TFII(cc,PII(nx,ny) ) );
		}
	}
}

double LNP::update_cost(int x, int y, double c){
	int dx[4] = { -1, 0, +1, 0 }, nx, ny;
	int dy[4] = { 0, +1, 0, -1 };
	VECTOR neighbors;

	for( int i=0; i<4; i++ ){
		nx = x + dx[i];
		ny = y + dy[i];
		if( nx >= 0 && nx < h &&
			ny >= 0 && ny < w ){
				neighbors.push_back( cost[nx][ny] );
			}
	}

	cnt++;
	cost[x][y] = wave_front_marcher(neighbors,0.1f);
	return cost[x][y];
}
